import java.util.ArrayList;

import lejos.robotics.navigation.Pilot;

/**
 * Stuurt robot constant aan met behulp van een steer.
 * Afstanden in cm;
 * @author s0200303
 *
 */
public class Driver{
	Pilot goldie;
	private float angle=0f;
	private float degrees=0f;
	private boolean completeCommand;
	private CommandEnum cmd = CommandEnum.IDLE;
	private ArrayList<Command> commandSequence;
	public long driverTimeout;
	Thread sp;
	final float moveSpeed;

	public Driver(Pilot pilot, int driverTimeout, float moveSpeed){
		this.goldie = pilot;
		this.driverTimeout = driverTimeout;
		this.moveSpeed = moveSpeed;
		commandSequence = new ArrayList<Command>();
		sp = new Thread(new SteerPilot()); 
		sp.start();
	}


	//	public void setCourse(float angle, float degrees, boolean completeCommand){
	//		this.angle = angle;
	//		this.degrees = degrees;
	//		this.completeCommand = completeCommand;
	//	}

	public void addCommand(Command c){
		if (c!=null){
			commandSequence.add(c);
		}
	}


	public void flushCommands(){
		commandSequence.clear();
	}


	class SteerPilot implements Runnable {
		float interruptProcent = 0.8f;
		public void run(){
			while (true){
				switch (cmd){
				case TRAVEL:
					if (degrees != 0)
					{
						goldie.travel(degrees/interruptProcent, true);
						//als travel volledig afgerond moet worden: yield thread tot travel compleet is
						if(completeCommand){
							float dist = goldie.getTravelDistance();
							while(Math.abs(goldie.getTravelDistance()-dist)<Math.abs(degrees))Thread.yield();
						}
					}
					break;

				case STEER:
					if(angle!=0){
						goldie.steer(angle,degrees/interruptProcent,true);
						//als bocht volledig afgerond moet worden: yield thread tot bocht compleet is
						if(completeCommand){
							float startAngle = goldie.getAngle();
							while(Math.abs(goldie.getAngle()-startAngle)<Math.abs(degrees))Thread.yield();
						}
					}
					break;

				case IDLE:
					goldie.stop();
					break;

				default:
					break;

				}

				if(!completeCommand){
					try{
						Thread.sleep(driverTimeout);
					}catch(InterruptedException e){}
				}
				fetchNextcommand();
			}
		}

		private void fetchNextcommand() {
			//completeCommand == false -> voer dit oneindig lang uit tenzij er een nieuw commando is
			//als laatste commando niet oneindig lang uitgevoerd moet worden en er geen commando meer is -> doe niets
			if (commandSequence.size() == 0 && completeCommand){
				cmd = CommandEnum.IDLE;
			}
			else if (commandSequence.size() != 0) {
				Command nextCommand;
				do{
					nextCommand = commandSequence.get(0);
					commandSequence.remove(0);
				}
				//ga naar het laatst gegeven commando tenzij completeCommand == false
				while(!nextCommand.isCompleteCommand()&& commandSequence.size() != 0);

				if(nextCommand!=null){
					angle = nextCommand.getAngle();
					degrees = nextCommand.getDegrees();
					completeCommand = nextCommand.isCompleteCommand();
					cmd = nextCommand.getCommandEnum();
				}
			}
		}
	}
}
